/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Velocity.h"

namespace Robotic { namespace Math{
	Velocity::Velocity(void)
	{
	}

	Velocity::Velocity(const Velocity& arg)
	{
		this->LinearVel = arg.LinearVel;
		this->AngleVel = arg.AngleVel;
	}

	//Velocity::Velocity(const ColumnVector& arg)
	//{
	//	if(arg.GetCount() != 6)
	//		throw(new std::exception("Argument arg's count must be 6."));

	//	this->LinearVel[0] = arg[0];
	//	this->LinearVel[1] = arg[1];
	//	this->LinearVel[2] = arg[2];
	//	this->AngleVel[0] = arg[3];
	//	this->AngleVel[1] = arg[4];
	//	this->AngleVel[2] = arg[5];

	//}

	Velocity::Velocity(const Vector3& linear,const Vector3& angle)
		:LinearVel(linear),AngleVel(angle)
	{

	}

	Velocity::~Velocity(void)
	{
	}

	double& Velocity::operator[](const int index)
	{
		if((index < 0) || (index > 5))
			throw(new std::exception("Argument row index must be between 0 and 5."));

		if (index<3) 
			return LinearVel[index];
		else
			return AngleVel[index-3];
	}

	double Velocity::operator[](const int index) const
	{
		if((index < 0) || (index > 5))
			throw(new std::exception("Argument row index must be between 0 and 5."));

		if (index<3) 
			return LinearVel[index];
		else
			return AngleVel[index-3];
	}

	Velocity& Velocity::operator = (const Velocity& arg)
	{
		LinearVel = arg.LinearVel;
		AngleVel = arg.AngleVel;
		return *this;
	}

	Velocity& Velocity::operator += (const Velocity& arg)
	{
		LinearVel += arg.LinearVel;
		AngleVel += arg.AngleVel;
		return *this;
	}

	Velocity& Velocity::operator -= (const Velocity& arg)
	{
		LinearVel -= arg.LinearVel;
		AngleVel -= arg.AngleVel;
		return *this;
	}

	bool operator == (const Velocity& lhs, const Velocity& rhs)
	{
		return Equal(lhs,rhs);
	}

	bool operator != (const Velocity& lhs, const Velocity& rhs)
	{
		return !Equal(lhs,rhs);
	}

	// addition of Velocity's
	Velocity operator + (const Velocity& lhs, const Velocity& rhs)
	{
		return Velocity(lhs.LinearVel + rhs.LinearVel, lhs.AngleVel + rhs.AngleVel);
	}

	// unary -
	Velocity operator - (const Velocity& lhs, const Velocity& rhs) 
	{
		return Velocity(lhs.LinearVel - rhs.LinearVel, lhs.AngleVel - rhs.AngleVel);
	}

	Velocity operator - (const Velocity& arg)
	{
		return Velocity( -arg.LinearVel, -arg.AngleVel );
	}

	Velocity operator * (const Velocity& lhs, const double rhs)
	{
		return Velocity(lhs.LinearVel * rhs, lhs.AngleVel * rhs);
	}

	Velocity operator * (const double lhs, const Velocity& rhs)
	{
		return Velocity(lhs * rhs.LinearVel, lhs * rhs.AngleVel);
	}

	double operator * (const Velocity& lhs,const Wrench& rhs) 
	{
		return lhs.LinearVel * rhs.Force + lhs.AngleVel * rhs.Moment;
	}

	Velocity operator / (const Velocity& lhs, const double rhs)
	{
		return Velocity(lhs.LinearVel / rhs, lhs.AngleVel / rhs);
	}


	void Velocity::SetZero( ) 
	{
		AngleVel.SetZero();
		LinearVel.SetZero();
	}

	Velocity Velocity::Zero()
	{
		return Velocity(Vector3::Zero(),Vector3::Zero());
	}

	void Velocity::ReverseSign()
	{   
		LinearVel.ReverseSign();
		AngleVel.ReverseSign();
	}

	Velocity Velocity::RefPoint(const Vector3& v_old_to_new) const
	{
		return Velocity(LinearVel + CrossProd(AngleVel, v_old_to_new), AngleVel);
	}

	Velocity Velocity::RefBaseFrame ( const Frame3& old_base_in_new_base) const
	{
		return Velocity(old_base_in_new_base.Rotation * LinearVel, old_base_in_new_base.Rotation * AngleVel);
	}

	bool Equal(const Velocity& a,const Velocity& b,const double eps) 
	{
		return (Equal(a.AngleVel,b.AngleVel,eps) && Equal(a.LinearVel,b.LinearVel,eps)  );
	}

	Velocity AddDelta(const Velocity& vel,const Acceleration& acc,const double dt) 
	{
		return Velocity(AddDelta(vel.LinearVel,acc.LinearAcc,dt),AddDelta(vel.AngleVel,acc.AngleAcc,dt));
	}	

	Acceleration Diff(const Velocity& a,const Velocity& b,const double dt) 
	{
		return Acceleration(Diff(a.LinearVel,b.LinearVel,dt),Diff(a.AngleVel,b.AngleVel,dt));
	}

	void Random(Velocity& a) 
	{
		Random(a.AngleVel);
		Random(a.LinearVel);
	}	

	void PosRandom(Velocity& a) 
	{
		PosRandom(a.AngleVel);
		PosRandom(a.LinearVel);
	}
}}
